/**
 * @file PlannerBase.cpp
 * @brief 路径规划器基类实现
 */

#include "core/PlannerBase.h"
#include <fstream>
#include <sstream>

namespace PathPlanning
{
PlannerBase::PlannerBase(std::shared_ptr<Map> map) : map_(map)
{
    // 设置默认分辨率
    if (map_)
    {
        config_             = std::make_shared<PlannerConfig>();
        config_->resolution = map_->getResolution();
    }
}

bool PlannerBase::loadConfig(const std::string& configFile)
{
    std::ifstream file(configFile);
    if (!file)
    {
        std::cerr << "Cannot open config file: " << configFile << std::endl;
        return false;
    }
    std::cout << "Loading config from: " << configFile << std::endl;

    std::string line;
    while (std::getline(file, line))
    {
        // 跳过空行和注释
        if (line.empty() || line[0] == '#')
        {
            continue;
        }

        std::istringstream iss(line);
        std::string        key;
        if (std::getline(iss, key, '='))
        {
            std::string value;
            if (std::getline(iss, value))
            {
                // 去除前后空格
                key.erase(0, key.find_first_not_of(" \t"));
                key.erase(key.find_last_not_of(" \t") + 1);
                value.erase(0, value.find_first_not_of(" \t"));
                value.erase(value.find_last_not_of(" \t") + 1);

                std::cout << "Config: " << key << " = " << value << std::endl;

                // 子类应该重写这个方法来解析具体参数
                // 这里只是基类实现
            }
        }
    }

    return true;
}

std::vector<Pose> PlannerBase::smoothPath(const std::vector<Pose>& path)
{
    if (path.size() <= 2)
    {
        return path;
    }

    std::vector<Pose> smoothed      = path;
    const int         numIterations = 100;
    const double      alpha         = 0.4; // 数据项权重
    const double      beta          = 0.2; // 平滑项权重

    for (int iter = 0; iter < numIterations; ++iter)
    {
        for (size_t i = 1; i < smoothed.size() - 1; ++i)
        {
            // 数据项 - 保持靠近原始路径
            double dataX = path[i].x - smoothed[i].x;
            double dataY = path[i].y - smoothed[i].y;

            // 平滑项 - 减小曲率
            double smoothX = smoothed[i - 1].x + smoothed[i + 1].x - 2 * smoothed[i].x;
            double smoothY = smoothed[i - 1].y + smoothed[i + 1].y - 2 * smoothed[i].y;

            // 更新位置
            smoothed[i].x += alpha * dataX + beta * smoothX;
            smoothed[i].y += alpha * dataY + beta * smoothY;
        }
    }

    return smoothed;
}

bool PlannerBase::validateStartGoal(const Point& start, const Point& goal)
{
    if (!map_->isInBounds(start))
    {
        std::cerr << "Start position is out of map bounds" << std::endl;
        return false;
    }

    if (!map_->isInBounds(goal))
    {
        std::cerr << "Goal position is out of map bounds" << std::endl;
        return false;
    }

    if (map_->isObstacle(start))
    {
        std::cerr << "Start position is in obstacle" << std::endl;
        return false;
    }

    if (map_->isObstacle(goal))
    {
        std::cerr << "Goal position is in obstacle" << std::endl;
        return false;
    }

    return true;
}

double PlannerBase::euclideanDistance(const Point& a, const Point& b) const
{
    return std::hypot(a.x - b.x, a.y - b.y);
}

double PlannerBase::manhattanDistance(const Point& a, const Point& b) const
{
    return std::abs(a.x - b.x) + std::abs(a.y - b.y);
}

std::vector<GridCell> PlannerBase::getNeighbors(const GridCell& cell, bool eightConnected)
{
    std::vector<GridCell> neighbors;

    // 4方向邻居
    int dx4[] = {0, 1, 0, -1};
    int dy4[] = {1, 0, -1, 0};

    // 8方向邻居 (包括对角线)
    int dx8[] = {0, 1, 1, 1, 0, -1, -1, -1};
    int dy8[] = {1, 1, 0, -1, -1, -1, 0, 1};

    if (eightConnected)
    {
        for (int i = 0; i < 8; ++i)
        {
            int nx = cell.x + dx8[i];
            int ny = cell.y + dy8[i];
            if (map_->isInBounds(nx, ny))
            {
                neighbors.emplace_back(nx, ny);
            }
        }
    }
    else
    {
        for (int i = 0; i < 4; ++i)
        {
            int nx = cell.x + dx4[i];
            int ny = cell.y + dy4[i];
            if (map_->isInBounds(nx, ny))
            {
                neighbors.emplace_back(nx, ny);
            }
        }
    }

    return neighbors;
}

} // namespace PathPlanning